//
// Created by cy on 22-5-4.
//
#include "calculate.hpp"
#include "except.hpp"

using namespace std;
using namespace spdlog;
using namespace cv;

class CalculatorPrivate {

public:
    explicit CalculatorPrivate(bool debug = false);

    bool m_debug;
    sutPreParam m_preParam{};
    sutNucData m_nucData{};
    sutCalculateConfigs m_calculateConfigs{};

    shared_ptr<spdlog::logger> m_outLogger;

    sutArmorData getFinalArmor(const ArmorDataVector &armorDataVector);

    sutArmorTravel getArmorTravel(const sutArmorData &armorData);

    sutNucData doNormalProduct(const sutArmorTravel &armorTravel);

    sutNucData doSentryProduct(const sutArmorTravel &armorTravel);

    sutNucData doTuoluoProduct(const sutArmorTravel &armorTravel);

    static tuple<float, float, float> pnpSolve(const array<Point2f, 4> &corners, enuArmorType armorType);

    static tuple<float, float> pointSolve(const Point2f &center);

    static tuple<int, int> unPointSolve(const Point2f &center);

    static float countGravityPitch(float x, float v, float g);

    static float countShootDelay(float v, float gravityPitch, float g);

    static float countPointDistance(const Point2f &point1, const Point2f &point2);

    static float countLineAngle(const Point2f &point1, const Point2f &point2);

    static float countSSIM(const sutArmorTravel travel1, const sutArmorTravel travel2);
};

CalculatorPrivate::CalculatorPrivate(bool debug) :
        m_debug(debug) {
    m_outLogger = stdout_color_st("CalculatorOut");
    m_outLogger->set_level(level::info);
    m_outLogger->flush_on(level::info);
}

Calculator::Calculator(const std::string &configFile, bool debug) :
        m_ptr(new CalculatorPrivate(debug))
{
    fstream fs;
    fs.open(configFile, ios::in);
    if (!fs.is_open()) {
        throw CecException("配置文件不存在，无法进行计算和预测!");
    } else {
        m_ptr->m_outLogger->info("配置文件已找到，加载配置文件中...");
        Json::CharReaderBuilder reader;
        Json::Value j;
        JSONCPP_STRING err;

        bool res = Json::parseFromStream(reader, fs, &j, &err);
        if (!res || !err.empty()) {
            throw CecException(fmt::format("加载配置文件时出现问题，错误原因->[{}]", err));
        } else {
            m_ptr->m_outLogger->info("配置文件加载成功!");
            try {
                m_ptr->m_calculateConfigs.offsetPitch = j["CAMERA_POSITION"]["offsetPitch"].asDouble();
                m_ptr->m_calculateConfigs.offsetYaw = j["CAMERA_POSITION"]["offsetYaw"].asDouble();
                m_ptr->m_calculateConfigs.gravity = j["CAMERA_POSITION"]["gravity"].asDouble();
                m_ptr->m_calculateConfigs.smallArmorWidth = j["ARMOR_SIZE"]["smallArmorWidth"].asDouble();
                m_ptr->m_calculateConfigs.smallArmorHeight = j["ARMOR_SIZE"]["smallArmorHeight"].asDouble();
                m_ptr->m_calculateConfigs.bigArmorWidth = j["ARMOR_SIZE"]["bigArmorWidth"].asDouble();
                m_ptr->m_calculateConfigs.bigArmorHeight = j["ARMOR_SIZE"]["bigArmorHeight"].asDouble();
                m_ptr->m_calculateConfigs.focalX = j["ARMOR_SIZE"]["focalX"].asDouble();
                m_ptr->m_calculateConfigs.focalY = j["ARMOR_SIZE"]["focalY"].asDouble();
                m_ptr->m_calculateConfigs.processNoise = j["ARMOR_SIZE"]["processNoise"].asDouble();
                m_ptr->m_calculateConfigs.measureNoise = j["ARMOR_SIZE"]["measureNoise"].asDouble();
            } catch (Json::Exception &e) {
                throw CecException(e);
            }
        }
    }
    fs.close();
}

Calculator::~Calculator() {
    delete m_ptr;
}

bool Calculator::update(const ArmorDataVector &armor, const sutPreParam &preParam) {
    m_ptr->m_preParam = preParam;
    m_ptr->m_nucData.isOk = false;
    if (!armor.empty()) {
        auto &&armorData = m_ptr->getFinalArmor(armor);
        auto &&armorTravel = m_ptr->getArmorTravel(armorData);
        if (preParam.predictFlag)
            m_ptr->m_nucData = m_ptr->doSentryProduct(armorTravel);
        else {
            if (preParam.outPutType == enuOutPutType::TYPE_TUOLUO) {
                m_ptr->m_nucData = m_ptr->doTuoluoProduct(armorTravel);
            } else {
                m_ptr->m_nucData = m_ptr->doNormalProduct(armorTravel);
            }
        }
    }
}

sutNucData &Calculator::result() {
    return m_ptr->m_nucData;
}

sutPreParam &Calculator::resultPreParam() {
    return m_ptr->m_preParam;
}

void Calculator::updateConfigs(const sutCalculateConfigs &configs) {

}

void Calculator::saveConfigs(const std::string &configFile) {

}

void Calculator::openDebug() {
    m_ptr->m_debug = true;
}

void Calculator::closeDebug() {
    m_ptr->m_debug = false;
}